#include "sys.h"
#include "taskB.h"
#include "app.h"

float y_B[] = {70.8,67.8};
float x_B = 39.6,W_B=50.0,B_B0 = 1.62;
int Ripe_B = -1;
extern u16 step;
//串口1为:树莓派/jetson nano
//串口2为:机械臂stm32f103
//串口3为:左openmv	
//串口4为:右openmv
//串口5为:前stm32f407(红外对管，超声波),f4蓝牙
//串口6为:小车stm32f103

void JudgeFruitRipe_B(uint8_t high) //0:地上，判断南瓜成熟 1：树上：判断辣椒成熟
{
	int r=0,i=0,j=0,k,num = 0;
	delay_ms(500);
	Ripe_B = -1;
	if(Fruit_num != 0)
	{
		for(j=0;j<10;j++)
		{
			if(high)
			{
				for(k=0;k<Fruit_num;k++)
				{
					if(fruit[k].object==3) num = k;
					u5_printf("pepper_num:%d\r\n",num);
				}
			}
			if(fruit[num].ripe == 1)
			{
				r++;
				u5_printf("r:%d\r\n",r);
			}
			else 
			{
				i++;
				u5_printf("i:%d\r\n",i);
			}
			delay_ms(100);
		}
		u5_printf("i=%d, r=%d\r\n", i, r);
		if(i>r)Ripe_B = 0;
		else Ripe_B = 1;
	}
	u5_printf("Ripe_B:%d\r\n", Ripe_B);	
}


void TaskB(void)
{
	static u16 step_B = 0;
	static u16 else_count = 0;
	if(stop_flag == 1)
	{
		switch(step_B)
		{
			case 0:
			{
				u5_printf("-------------1------------\r\n");
				direction = 1; //右边果树			
				order(3, 'S'); //让车停下
				wait_w(3, 'S', 1);
				u5_printf("STOP\r\n");
				u5_printf("find target\r\n");
				if(fabs(Leida_Distance[2]-B_B0)>0.3) CloseToWall(-1,-1,B_B0);
				AB0_CloseToFruit(x_B,y_B[direction]);
				step_B++;
				break;
			}
			case 1:
			{
				JudgeFruitRipe_B(0);
				if(Ripe_B==1)		GrapLowFruit_AB('L');
				step_B++;
				break;
			}
			case 2:
			{
				order_num(2, 'D', 1);					//看树上
				AB1_CloseToFruit(W_B);
				JudgeFruitRipe_B(1);
				if(Ripe_B==1)	GrapHighFruit_AB(1);
				step_B++;
				break;
			}
			case 3:
			{	 
				else_count = 0;
				stop_flag = 0;//跳出if进入else
				step_B++;
				break;
			}
			case 4:
			{
				u5_printf("-------------2------------\r\n");
				direction = 0;//左边果树				
				order(3, 'S'); //让车停下
				wait_w(3, 'S', 1);
				u5_printf("STOP\r\n");
				u5_printf("find target\r\n");
				if(fabs(Leida_Distance[2]-B_B0+1)>0.3) CloseToWall(-1,-1,B_B0+1);
				AB0_CloseToFruit(x_B,y_B[direction]);
				step_B++;
				break;
			}
			case 5:
			{
				JudgeFruitRipe_B(0);
				if(Ripe_B==1)		GrapLowFruit_AB('R');
				step_B++;
				break;
			}
			case 6:
			{
				order_num(2, 'A', 1);					//看树上
				AB1_CloseToFruit(W_B);
				JudgeFruitRipe_B(1);
				if(Ripe_B==1)	GrapHighFruit_AB(1);
				step_B++;
				break;
			}
			case 7:
			{
				else_count = 0;
				stop_flag = 0;//跳出if进入else
				step_B++;
				break;
			}
			case 8:
			{
				u5_printf("-------------3------------\r\n");
				direction = 1; //右边果树
				order(3, 'S'); //让车停下
				wait_w(3, 'S', 1);
				u5_printf("STOP\r\n");
				u5_printf("find target\r\n");
				if(fabs(Leida_Distance[2]-B_B0+2)>0.3) CloseToWall(-1,-1,B_B0+2);
				AB0_CloseToFruit(x_B,y_B[direction]);
				step_B++;
				break;
			}
			case 9:
			{
				JudgeFruitRipe_B(0);
				if(Ripe_B==1)	GrapLowFruit_AB('L');
				step_B++;
				break;
			}
			case 10:
			{
				order_num(2, 'D', 1);					//看树上
				AB1_CloseToFruit(W_B);
				JudgeFruitRipe_B(1);
				if(Ripe_B==1)	GrapHighFruit_AB(1);
				step_B++;
				break;
			}
			case 11:
			{
				u5_printf("case 11\r\n");
				u5_printf("LOOK UP\r\n");
				order_num(2, 'K', 1);
				step_B++;
				else_count = 0;
				u5_printf("reset else_count:%d\r\n", else_count);
				stop_flag = 0;//跳出if进入else
				direction = 0;
				step++;
				return;
			}
		}
	}
	else
	{
		if(else_count == 0)
		{
			u5_printf("the first else\r\n");
			if(direction == 0)
			{
				order_num(2, 'A', 0);
			}
			else if(direction == 1)
			{
				order_num(2, 'D', 0);
			}
			if(step_B==0) u3_printf("~%c%.2f~%c%.2f\r\n",'X',0.5,'W',0.15);			
			else u3_printf("~%c%.2f~%c%.2f\r\n",'X',1.0,'W',0.15);	//浮点数输出必须要有小数点 %f和%lf输出6位小数	
			wait_s();
			if(step_B==0)	if(fabs(Leida_Distance[2]-B_B0)>0.3) CloseToWall(-1,-1,B_B0);		
			stop_flag = 0;
		}
		else
		{
			if(Fruit_num != 0)
			{
				u5_printf("Fruit_num:%d\r\n",Fruit_num);
				stop_flag = 1;
			}
		}
		else_count = 1;
	}
}

